#include "can_motor.h"

CAN_Fback_Inf canMotor_inf[8];
CAN_MotorSupport_t CAN_MSupport[sizeof(canMotor_inf) / sizeof(CAN_Fback_Inf)];

/**
 * @brief  can1电机发送电流
 * @param  void
 * @retval void
 */
void send_can1_motor_current(void)
{
	uint8_t can1_tx_buffer[8];

	can1_tx_buffer[0] = canMotor_inf[0].control_current >> 8; 
	can1_tx_buffer[1] = canMotor_inf[0].control_current; 

	can1_tx_buffer[2] = canMotor_inf[1].control_current >> 8; 
	can1_tx_buffer[3] = canMotor_inf[1].control_current; 
	
	can1_tx_buffer[4] = canMotor_inf[2].control_current >> 8; 
	can1_tx_buffer[5] = canMotor_inf[2].control_current; 
	
	can1_tx_buffer[6] = canMotor_inf[3].control_current >> 8;
	can1_tx_buffer[7] = canMotor_inf[3].control_current;
	
	can1_SendPacket(can1_tx_buffer, 0x200);
	
//	can1_tx_buffer[0] = canMotor_inf[4].control_current >> 8; 
//	can1_tx_buffer[1] = canMotor_inf[4].control_current; 

//	can1_tx_buffer[2] = canMotor_inf[5].control_current >> 8; 
//	can1_tx_buffer[3] = canMotor_inf[5].control_current; 
//	
//	can1_tx_buffer[4] = canMotor_inf[6].control_current >> 8; 
//	can1_tx_buffer[5] = canMotor_inf[6].control_current; 
//	
//	can1_tx_buffer[6] = canMotor_inf[7].control_current >> 8; 
//	can1_tx_buffer[7] = canMotor_inf[7].control_current; 
//	
//	can1_SendPacket(can1_tx_buffer,0X1ff);
}	

/**
 * @brief  can2电机发送电流
 * @param  void
 * @retval void
 */
void send_can2_motor_current(void)
{
//	uint8_t can2_tx_buffer[8];
//	
//	can2_tx_buffer[0] = canMotor_inf[8].control_current >> 8; 
//	can2_tx_buffer[1] = canMotor_inf[8].control_current; 

//	can2_tx_buffer[2] = canMotor_inf[9].control_current >> 8; 
//	can2_tx_buffer[3] = canMotor_inf[9].control_current; 
//	
//	can2_tx_buffer[4] = canMotor_inf[10].control_current >> 8; 
//	can2_tx_buffer[5] = canMotor_inf[10].control_current; 
//	
//	can2_tx_buffer[6] = canMotor_inf[11].control_current >> 8; 
//	can2_tx_buffer[7] = canMotor_inf[11].control_current; 

//	can2_SendPacket(can2_tx_buffer, 0x200);
//	
//	can2_tx_buffer[0] = canMotor_inf[12].control_current >> 8; 
//	can2_tx_buffer[1] = canMotor_inf[12].control_current; 

//	can2_tx_buffer[2] = canMotor_inf[13].control_current >> 8; 
//	can2_tx_buffer[3] = canMotor_inf[13].control_current; 
//	
//	can2_tx_buffer[4] = canMotor_inf[14].control_current >> 8; 
//	can2_tx_buffer[5] = canMotor_inf[14].control_current; 
//	
//	can2_tx_buffer[6] = canMotor_inf[15].control_current >> 8; 
//	can2_tx_buffer[7] = canMotor_inf[15].control_current; 
//	can2_SendPacket(can2_tx_buffer,0X1ff);
}	

/**
 * @brief  电机解析出对应的数据
 * @param  void
 * @retval void
 */
void can_motor_measure(uint8_t id, uint8_t data[])
{
	CAN_MSupport[id].last_angle = canMotor_inf[id].angle;
//	CAN_MSupport[id].frist_flag = 0;
	
	/* 角度及速度 */
	canMotor_inf[id].angle = data[0] << 8 | data[1];
	canMotor_inf[id].speed = data[2] << 8 | data[3];
	
	/* 电流及温度 */
	canMotor_inf[id].torque_current = (data[4] << 8 | data[5]);
	canMotor_inf[id].temperature = data[6];

	if(canMotor_inf[id].angle - CAN_MSupport[id].last_angle > 4096){
		CAN_MSupport[id].round_cnt--;
	}	else if(canMotor_inf[id].angle - CAN_MSupport[id].last_angle < -4096){
		CAN_MSupport[id].round_cnt++;
	}
	
	canMotor_inf[id].total_angle = CAN_MSupport[id].round_cnt * 8192 + canMotor_inf[id].angle - CAN_MSupport[id].fris_angle;
}

void can_motor_offset(uint8_t id, uint8_t data[])
{	
	canMotor_inf[id].angle = data[0] << 8 | data[1];
	CAN_MSupport[id].fris_angle = canMotor_inf[id].angle;
}


void can_motor_updata(uint32_t StdId, uint8_t *rxbuff)
{
  static uint32_t id = 0;
	
	/* 获取返回数据属于哪个ID */
	id = StdId - CAN_Motor_ID1;
	
	/* 是否上电 */
	if (CAN_MSupport[id].frist_cnt < 50) {
		CAN_MSupport[id].frist_cnt++;
		can_motor_offset(id,rxbuff);
	} else {
		can_motor_measure(id,rxbuff);
	}
}

/**
 * @brief  设置电机电流 
 * @param  void
 * @retval void
 */
void CAN_Motor_setCurrent(CAN_Message_ID _ID, int16_t current)
{
	uint16_t CAN_ID = _ID - CAN_Motor_ID_Base;
	
	canMotor_inf[CAN_ID].control_current = current;	
}

/**
 * @brief  获取电机电流 
 * @param  void
 * @retval void
 */
uint16_t CAN_Motor_getCurrent(CAN_Message_ID _ID)
{
	uint16_t CAN_ID = _ID;
	
	return canMotor_inf[CAN_ID - CAN_Motor_ID_Base].control_current;	
}

/**
 * @brief  获取电机速度
 * @param  void
 * @retval void
 */
int16_t CAN_Motor_getSpeed(CAN_Message_ID _ID)
{
	int16_t speed;
	uint16_t CAN_ID = _ID;
	
	speed = canMotor_inf[CAN_ID - CAN_Motor_ID_Base].speed;	
	return speed;
}

/**
 * @brief  获取电机角度
 * @param  void
 * @retval void
 */
int16_t CAN_Motor_getAngle(CAN_Message_ID _ID)
{
	int16_t angle;
	uint16_t CAN_ID = _ID;
	
	angle = canMotor_inf[CAN_ID - CAN_Motor_ID_Base].angle;	
	return angle;
}

/**
 * @brief  获取电机总距离
 * @param  void
 * @retval void
 */
int32_t CAN_Motor_getTotalAngle(CAN_Message_ID _ID)
{
	int32_t total_angle;
	uint16_t CAN_ID = _ID;
	
	total_angle = canMotor_inf[CAN_ID - CAN_Motor_ID_Base].total_angle;
	
	return total_angle;
}
